﻿using KSASupport;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using KSASupport.BlobTracking;

namespace KSATouch
{
    /// <summary>
    /// This class is responsible for finding out details about blobs, such as tilt, type, side, etc.
    /// Will be used to find hands, sides arms are on, objects, orientation (xy z), and height
    /// </summary>
    public class BlobAnalyzer
    {
        // Tracks how far away a blob can be from a bounding box to be considered 'valid'. 
        public const int DistanceThreshold = 3;

        /// <summary>
        /// Analyzes blobs passed to it, and classifies them as 'arms, fingers, objects, etc'
        /// </summary>
        public BlobAnalyzer() { }

        /// <summary>
        /// Responsible for detecting arm contacts vs. object contacts in the hover layer.
        /// </summary>
        /// <param name="blobs"></param>
        /// <returns>A list of Contacts which are noted as 'arms'</returns>
        public List<DepthBlob> ProcessHoverLayer(List<DepthBlob> hoveringBlobs)
        {
            List<DepthBlob> joinedBlobs = JoinNearBlobs(hoveringBlobs, DistanceThreshold);
            return joinedBlobs;
        }

        /// <summary>
        /// Joins blobs which are within a certain range of each other.        
        /// </summary>
        /// <param name="?"></param>
        /// <param name="radius"></param>
        /// <returns></returns>
        public List<DepthBlob> JoinNearBlobs(List<DepthBlob> blobs,double radius)
        {
            foreach (DepthBlob blob in blobs)
            {
                if (!blob.IsValid)
                {
                    continue;
                }
                blob.BoundingBox = Rect.Inflate(blob.BoundingBox, radius, radius);
                foreach (DepthBlob blobInner in blobs)
                {                    
                    if (blob.Id != blobInner.Id) // Filter out 'self'
                    {
                        blobInner.BoundingBox = Rect.Inflate(blobInner.BoundingBox, radius, radius);
                        if(blobInner.BoundingBox.IntersectsWith(blob.BoundingBox))
                        {
                            // Promote blob over blobInner
                            blob.Points.AddRange(blobInner.Points);
                            blob.Update();
                            blobInner.IsValid = false;
                        }
                    }
                }
            }
            blobs.RemoveAll(bl => bl.IsValid == false);
            return blobs;
        }

        /// <summary>
        /// This will process the blobs in the depth frame. It will update blobs that seem to be 'fingers', as well as join large blobs which intersect
        /// </summary>
        /// <param name="e"></param>
        /// <returns></returns>
        internal List<Contact> ProcessDepthFrame(ref DepthManager depthManager)
        {
            if (depthManager == null)
                return new List<Contact>();

            List<DepthBlob> hoveringBlobs  = depthManager.HoveringBlobs;
            List<DepthBlob> touchBlobs = depthManager.DepthBlobs;

            // Phase one: Find 'furthest point'. Store this off for further use..this is most likely an oustretched finger
            List<Contact> depthContacts = new List<Contact>();
            foreach (DepthBlob blob in hoveringBlobs)
            {
                // Skip invalid blobs
                if (!blob.IsValid)
                {
                    continue;
                }
                Contact contact = new Contact(blob);

                // Map the centerpoint based on the touch mapping value
                contact.CenterPoint = depthManager.Warper.Warp((float)blob.CenterPoint.X, (float)blob.CenterPoint.Y);
                
                contact.Id = Contact.CurrentContactID;
                // Find any intersecting rectangles. If found, mark them as invalid, and merge them with this one
                foreach (DepthBlob depthBlob in hoveringBlobs)
                {
                    if ((depthBlob.Id != blob.Id) && (depthBlob.BoundingBox.IntersectsWith(blob.BoundingBox)))
                    {
                        depthBlob.IsValid = false;
                        blob.Points.AddRange(depthBlob.Points);
                        blob.Update();
                    }
                }

                depthContacts.Add(contact);
            }
            hoveringBlobs.RemoveAll(bl => bl.IsValid == false);
            
            // Phase 2: screen contacts which are not near the bounding box of a hovering contact get filtered out (this should remove 'false contacts'
            foreach (DepthBlob blob in touchBlobs)
            {
                // flip valid flag..we're going to make the blobs prove they deserve to be here :)
                blob.IsValid = false;
                
                //
                // TODO: This has a bad bug. we should check the rectangles orientation,  For now it's 'good enough'
                //
                foreach(DepthBlob hoveringBlob in hoveringBlobs)
                {
                    Rect boundaryRect = depthManager.DepthManagerSettings.ScreenBoundaryRect;
                    if (hoveringBlob.BoundingBox.Top < boundaryRect.Top+20)
                    {
                        hoveringBlob.Side = BlobSide.Top;                            
                    }
                    if (hoveringBlob.BoundingBox.Left < boundaryRect.Left+20)
                    {
                        hoveringBlob.Side = BlobSide.Right;
                    }
                    if (hoveringBlob.BoundingBox.Right > boundaryRect.Right-20)
                    { 
                        hoveringBlob.Side = BlobSide.Left;
                    }
                    if (hoveringBlob.BoundingBox.Bottom > boundaryRect.Bottom-20)
                    {
                        hoveringBlob.Side = BlobSide.Bottom;
                    }
                }

                foreach (DepthBlob hoveringBlob in hoveringBlobs)
                {
                    // blobs inside of bounding boxes which have 'side != none' are fingers under hands. They're given a pass
                    if (hoveringBlob.Side != BlobSide.None)
                    {
                        if (hoveringBlob.BoundingBox.Contains(blob.CenterPoint))
                        {
                            blob.IsValid = true;
                            blob.Side = hoveringBlob.Side; // should assign this blob to the hovering blob
                            continue;
                        }
                    }
                    double distance = double.MaxValue;
                    double closestDistance = double.MaxValue;
                    DepthBlob closestThing = null;

                    // measure against all bounding boxes. If its close, find out if the bounding box has a 'side' (a hand). If not, blend blobs into an 'object'
                    if ((blob.CenterPoint.X < hoveringBlob.BoundingBox.Right) && (blob.CenterPoint.X > hoveringBlob.BoundingBox.Left))
                    {
                        // Measure top vs bottom
                        distance = Math.Abs(blob.CenterPoint.Y - hoveringBlob.BoundingBox.Top);
                        if ((distance < DistanceThreshold) && (distance < closestDistance))
                        {
                            closestDistance = distance;
                            closestThing = hoveringBlob;
                        }
                        distance = Math.Abs(blob.CenterPoint.Y - hoveringBlob.BoundingBox.Bottom);
                        if ((distance < DistanceThreshold) && (distance < closestDistance))
                        {
                            closestDistance = distance;
                            closestThing = hoveringBlob;
                        }
                    }
                    if ((blob.CenterPoint.X < hoveringBlob.BoundingBox.Right) && (blob.CenterPoint.X > hoveringBlob.BoundingBox.Left))
                    {
                        // measure right vs. left
                        distance = Math.Abs(blob.CenterPoint.X - hoveringBlob.BoundingBox.Right);
                        if ((distance < DistanceThreshold) && (distance < closestDistance))
                        {
                            closestDistance = distance;
                            closestThing = hoveringBlob;
                        }
                        distance = Math.Abs(blob.CenterPoint.X - hoveringBlob.BoundingBox.Left);
                        if ((distance < DistanceThreshold) && (distance < closestDistance))
                        {
                            closestDistance = distance;
                            closestThing = hoveringBlob;
                        }
                    }
                    if ((closestDistance < DistanceThreshold))
                    {
                        blob.IsValid = true;
                        if (closestThing.Side != BlobSide.None)
                        {
                            blob.Side = closestThing.Side;
                        }
                        if (closestThing.Side == BlobSide.None)
                        {
                            // this blob is an object, or belongs to an object. Let the object 'absorb' these points.
                            //blob.IsValid = false;
                            //closestThing.Points.AddRange(blob.Points);
                        }
                        else
                        {
                            blob.IsValid = true;                            
                        }

                    }
                }
            }
            // cleanup blobs which are not near a hand
            touchBlobs.RemoveAll(bl => bl.IsValid == false);

            // Phase 3: Combine finger blobs which are 'close'. This combats the noise in a way
            // TODO: Cleanup...refactor this into a method, and use it for the hands and the fingers of the image (possibly objects as well?)
            foreach (DepthBlob fingerBlob in touchBlobs)
            {
                // Skip invalid blobs
                if (!fingerBlob.IsValid)
                {
                    continue;
                }
                
                // Find any intersecting rectangles. If found, mark them as invalid, and merge them with this one
                foreach (DepthBlob fingerBlob2 in touchBlobs)
                {
                    if ((fingerBlob2.Id != fingerBlob.Id) && (fingerBlob2.BoundingBox.IntersectsWith(fingerBlob.BoundingBox)))
                    {
                        fingerBlob2.IsValid = false;
                        fingerBlob.Points.AddRange(fingerBlob2.Points);
                        fingerBlob.Update();
                    }
                }
            }
            // cleanup for any contacts which no longer should matter
            depthContacts.RemoveAll(dc => dc.IsValid == false);

            // cleanup for any remaining blobs
            hoveringBlobs.RemoveAll(bl => bl.IsValid == false);
            touchBlobs.RemoveAll(bl => bl.IsValid == false);
            return depthContacts;
        }
    }
}
